/******************************************************************
简介：实现Mega2560扩展版控制4自由度舵机

接线: 四自由度机械臂从上到下舵机引脚
              爪子: 舵机引脚：4
               |
          手腕关节1：舵机引脚：6
               |
              小臂：舵机引脚：7
               |
              大臂：舵机引脚：10
               |
              转台：舵机引脚：11


日期：2024.11.29
QQ交流群：811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司.
******************************************************************/

#include "uni_servo.h"

//注意，此方法目前最大支持20个舵机，后期会增加到最大支持50个舵机
UNI_SERVO_S servo(4, 6, 7, 10, 11); //声明4,6,7,10,11引脚舵机

void setup(){
  delay(1000);
  Serial.begin(115200);
  servo.init(); //舵机引脚初始化设置
  /**********************************************************
  servo.move(int angle1, int angle2, int angle3, int angle4, int angle5);
    angle1:爪子舵机张开的角度
    angle2:手腕关节舵机1角度
    angle3:小臂舵机角度
    angle4:大臂舵机角度 
    angle5:转台舵机角度  */
  servo.move(90, 90, 90, 90, 90); //机械臂转动到初始角度
  delay(1000);
}

void loop(){
  servo.move(90, 90, 90, 90, 90);     //机械手臂保持初始动作
  delay(1000);
  servo.move(90, 178, 178, 90, 90);   //机械手臂转动到要抓取物体的状态
  delay(1000);
  servo.move(178, 178, 178, 90, 90);  //机械手臂抓取物体
  delay(1000);
  servo.move(178, 90, 90, 90, 90);    //机械手臂回复到初始角度，爪子保持闭合状态
  delay(1000);
  servo.move(178, 178, 178, 90, 176); //机械手臂转动到要放置物体的位置
  delay(1000);
  servo.move(90, 178, 178, 90, 176);  //机械手臂放置物体
  delay(1000);
}
